//
// Created by weimian on 2021/3/15.
//

#include "io/las_reader.h"

void LasReader::read(const std::string &path, pcl::PointCloud<pcl::PointXYZRGB> &cloud) {
    if (!cloud.empty()) {
        return;
    } else {
        // 打开LAS格式点云文件
        std::ifstream ifs(path, std::ios::in | std::ios::binary);
        liblas::ReaderFactory f;
        liblas::Reader reader = f.CreateWithStream(ifs);

        // 读取LAS文件信息头
        liblas::Header const &header = reader.GetHeader();
        int nbPoints = header.GetPointRecordsCount();

        cloud.width = nbPoints;
        cloud.height = 1;
        cloud.is_dense = false;
        cloud.points.resize(cloud.width * cloud.height);

        int i = 0;
        uint8_t r, g, b;

        // 读取点云坐标和色彩信息
        while (reader.ReadNextPoint()) {
            // 获取las数据的x，y，z信息
            cloud.points[i].x = boost::lexical_cast<float>(reader.GetPoint().GetX());
            cloud.points[i].y = boost::lexical_cast<float>(reader.GetPoint().GetY());
            cloud.points[i].z = boost::lexical_cast<float>(reader.GetPoint().GetZ());

            // 获取las数据的r，g，b信息
            r = reader.GetPoint().GetColor().GetRed();
            g = reader.GetPoint().GetColor().GetGreen();
            b = reader.GetPoint().GetColor().GetBlue();

            std::uint32_t rgb = ((std::uint32_t)r << 16 | (std::uint32_t)g << 8 | (std::uint32_t)b);
            cloud.points[i].rgb = *reinterpret_cast<float *>(&rgb);

            i++;
        }
    }
}